/*
 * TITLE:       main.cc (symbolic link to main.c so Glade is happy)
 *
 * PURPOSE:     This is the main entry point into the CMVision Editor. It creates
 *              the gui, processes the command line and starts running CMVision on
 *              the default video channel.
 *
 *              To edit the window appearance run glade cmveditor.glade and generate
 *              the code once the changes are made. These will be added to the end
 *              of the callbacks files.
 *
 * WRITTEN BY:  Brett Browning, modified from {James Bruce} Small Size code 2001
 * (c) Carnegie Mellon University
 *
 * REVISION HISTORY:
 * 10 Oct 01 - created using glade and Jim's old code
 */


/*
 * Initial main.c file generated by Glade. Edit as required.
 * Glade will not overwrite this file.
 */

#ifdef HAVE_CONFIG_H
#  include <config.h>
#endif

#include <gtk/gtk.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <time.h>
#include <getopt.h>

#include <pthread.h>
#include <semaphore.h>

#include "interface.h"
#include "support.h"
#include "main.h"

#include "array3d.h"
#include "colors.h"
//#include "image.h"

#include "reality/cmvision/cmvision.h"

#include "threshold.h"
#include "constants.h"
#include "draw.h"

#ifdef CAPTURE
#include "reality/cmvision/capture.h"
#endif


typedef void * (*thread)(void *);

/************************************* TYPES *************************************/

// default file names -- default run from bin
// need to use F180CONFIG for this
#define COLORS_FNAME     "colors.txt"
#define THRESH_FNAME     "thresh.tmap"
#define DEFAULT_DEVICE   "/dev/video0"

// temp here
struct example{
  uchar y,u,v;  // YUV color
  uchar c;      // color class
  example *next;
};


typedef array3d_def<example*> emap_t;
typedef array3d_def<rgb> hist_t;

// maximum filename + path
#define MAX_PATH        1024

#define MODE_VIDEO 0
#define MODE_CLASS 1



/************************************* GLOBALS ***********************************/
GtkWidget *statusbar, *cmapstatusbar;
GtkWidget *colorfile, *paramsfile;
GtkWidget *cmveditor, *colormapwin, *display;
GtkWidget *cmapdarea, *image_area;

//Threshold threshold;

#ifdef CAPTURE
capture cap;
int image_width,image_height;
#endif


emap_t emap;
#ifdef CAPTURE
hist_t hist;
#endif

CMVision::color_class_state color[MAX_COLORS];
int num_colors;


int level,current_color;
bool run;
char filename[MAX_PATH] = THRESH_FNAME;
char device[MAX_PATH] = DEFAULT_DEVICE;
int width,height,zoom;
int last_x,last_y;

// drawing data
DrawHistogram dhistogram;
DrawRGB drawrgb(IMAGE_WIDTH, IMAGE_HEIGHT);
CParamDraw dparam;

rgb rawimg[IMAGE_WIDTH * IMAGE_HEIGHT];
rgb colorimg[IMAGE_WIDTH * IMAGE_HEIGHT];
uyvy yuvimg[IMAGE_WIDTH * IMAGE_HEIGHT];

bool grabbing = false, grabone = true;
bool showsegmentation = false;
bool running = true;

sem_t mutex;
pthread_t thread_id;
guint timeout_id;

int drawcount = 0, nextdrawcount = 0;

char configdir[MAX_PATH];

/************************************* PROTOTYPES ********************************/
gint timer(gpointer data);
void InitCapture();

void capture_thread(void *data);

#ifdef CAPTURE
void StopCapture();
yuv TMapLoc(yuv p);
yuv GetLocation(int x,int y);
void UpdateHistogram();
#endif


/************************************* CODE **************************************/


/*
 * main -
 *
 * This is the main entry point to the code. It processes the command line (after GTK 
 * has had a go at it. It starts CMVision capturing images on the default capture
 * device, with the default options.
 *
 * RETURN VALUE: 0 on success, error code on failure
 */
int main (int argc, char *argv[])
{
//  int x,y,b;
//  bool redraw_tmap,redraw_view;
//  yuv p;
  char c;

  /* process teh user options 
   * -d <device name>
   * -f <threshold map>
   */
  while((c = getopt(argc, argv, "d:f:h")) != EOF) {
    switch (c) {
    case 'd': strcpy(device, optarg); break;
    case 'f': strcpy(filename, optarg); break;
    case 'h': 
      printf("Usage: cmvedit -[df]\n");
      printf("\t-d <device name>   - use the video device specified\n");
      printf("\t-f <filename>   - use the color file specified\n");
      exit(1);
      break;
    default: 
      fprintf(stderr, "%s: Unknown option -%c.", argv[0], c);
      exit(1);
    }
  }

  printf("file %s device %s\n", filename, device);

  // get the config directory
  strcpy(configdir, getenv("F180CONFIG"));
  if(!configdir) 
    strcpy(configdir, ".");
  printf("Configuration directory = %s\n", configdir);


  /* initialize GTK and its subsystems */
  gtk_set_locale ();
  gtk_init (&argc, &argv);
  gdk_rgb_init();

  /* define the pixmap directory */
  add_pixmap_directory ( "/pixmaps");
  add_pixmap_directory ( "/pixmaps");

  /*
   * The following code was added by Glade to create one of each component
   * (except popup menus), just so that you see something after building
   * the project. Delete any components that you don't want shown initially.
   */
  cmveditor = create_cmveditor ();
  gtk_widget_show (cmveditor);

  colormapwin = create_colormapwin();
  display = create_display();
  gtk_widget_show (display);
  gtk_widget_show (colormapwin);

  /* save of the widgets for the status bar */
  statusbar = lookup_widget(cmveditor, "statusbar");
  cmapstatusbar = lookup_widget(colormapwin, "colorstatusbar");
  cmapdarea = lookup_widget(colormapwin, "cmap_darea");  
  colorfile = lookup_widget(cmveditor, "colorfile");
  paramsfile = lookup_widget(cmveditor, "paramsfile");
  image_area = lookup_widget(display, "image_area");
  drawrgb.SetWidget(image_area);

  /* now load the color information */
  char fname[1024];

  sprintf(fname, "%s/%s", configdir, COLORS_FNAME);
  num_colors = LoadColorInformation(color, MAX_COLORS, fname);
  fprintf(stderr, "Loaded %d Colors\n", num_colors);

  /* initialize the graphics interfaces */
  fprintf(stderr, "Initialize\n");
  dhistogram.Initialize(cmapdarea, num_colors, color);

  // initialize the calibration map like this...
  dparam.Initialize(image_area);


  //  class.Initialize(image_area, stuff...);

  sprintf(fname, "%s/%s", configdir, filename);
  if (!dhistogram.Load(fname)) {
    fprintf(stderr, "New file.\n");
    dhistogram.ResetMap();
  }

  // Load a reverse index table / histogram
  emap.init(MAX_YSIZE, MAX_USIZE, MAX_VSIZE);
  emap.set(NULL);
  emap.setDefault(NULL);
  //  if(argc >= 4) LoadHistogramFile(emap,argv[2]);

  // intialize the mutex semaphore for the drawing
  sem_init(&mutex, 0, 1);

  // need to use this


  /* intiailsie the capture */
  if (device) 
    InitCapture();
  // TestHistogram();
  // dummy for now
  timeout_id = gtk_timeout_add(50, (GtkFunction) timer, NULL);

//  if (pthread_create(&thread_id, NULL, (thread) capture_thread, NULL)) {
//    fprintf(stderr, "ERROR: Cannot create capture thread\n");
//    exit(1);
//  }

  dhistogram.Draw();


  /*  lets do it baby */
  gtk_main();

  // wait for other thread
  running = false;
  pthread_join(thread_id, NULL);

  return 0;
}

// to be called from the GTK thread
void Quit(void)
{
#ifdef DEBUG
  fprintf(stderr, "quiting\n");
#endif

  running = false;
  grabbing = false;
  gtk_timeout_remove(timeout_id);
  pthread_join(thread_id, NULL);
  gtk_main_quit();
}

// kind of messy but no easy way to synchronise thread with gtk and X
gint timer(gpointer data)
{
  drawcount++;
  if (grabbing || grabone || (dparam.isClicked && (nextdrawcount < drawcount))) {

    UpdateHistogram();

//    sem_wait(&mutex);
    dhistogram.Redraw();
//    sem_post(&mutex);

    // why are we calling this here???
    dparam.SetSize(IMAGE_WIDTH, IMAGE_HEIGHT);

    char *iptr = (showsegmentation ? (char *) rawimg : (char *) colorimg);
    if (dparam.isShowing)
      dparam.Draw(iptr);
    drawrgb.Draw(iptr);

    if (dparam.isClicked)
      nextdrawcount = drawcount + 5;
  }
  grabone = false;
  return (1);
}

void capture_thread(void *data)
{
  while (running) {
    if (grabbing) {
//      sem_wait(&mutex);
//      UpdateHistogram();
//      sem_post(&mutex);

      sleep(0);
    }
  }
}


//====================================================================//

unsigned int matrix_coefficients = 6;
const signed int Inverse_Table_6_9[8][4] =
{
    {117504, 138453, 13954, 34903}, /* no sequence_display_extension */
    {117504, 138453, 13954, 34903}, /* ITU-R Rec. 709 (1990) */
    {104597, 132201, 25675, 53279}, /* unspecified */
    {104597, 132201, 25675, 53279}, /* reserved */
    {104448, 132798, 24759, 53109}, /* FCC */
    {104597, 132201, 25675, 53279}, /* ITU-R Rec. 624-4 System B, G */
    {104597, 132201, 25675, 53279}, /* SMPTE 170M */
    {117579, 136230, 16907, 35559}  /* SMPTE 240M (1987) */
};


unsigned char * table_rV[256];
unsigned char * table_gU[256];
int table_gV[256];
unsigned char * table_bU[256];

static int div_round (int dividend, int divisor){
  if (dividend > 0)
    return (dividend + (divisor>>1)) / divisor;
  else
    return -((-dividend + (divisor>>1)) / divisor);
}

static void yuv_to_rgb_init()
{
  int i;
  unsigned char table_Y[1024];
  unsigned char  *table_8 = 0;
  unsigned int entry_size = 0;
  unsigned char  *table_r = 0, *table_g = 0, *table_b = 0;

  int crv = Inverse_Table_6_9[matrix_coefficients][0];
  int cbu = Inverse_Table_6_9[matrix_coefficients][1];
  int cgu = -Inverse_Table_6_9[matrix_coefficients][2];
  int cgv = -Inverse_Table_6_9[matrix_coefficients][3];

  for (i = 0; i < 1024; i++){
    int j;
    j = (76309 * (i - 384 - 16) + 32768) >> 16;
    j = (j < 0) ? 0 : ((j > 255) ? 255 : j);
    table_Y[i] = j;
  }

  table_8 = (unsigned char*) malloc ((256 + 2*232) * sizeof (unsigned char));

  entry_size = sizeof (unsigned char);
  table_r = table_g = table_b = table_8 + 232;

  for (i = -232; i < 256+232; i++)
    ((unsigned char * )table_b)[i] = table_Y[i+384];

  for (i = 0; i < 256; i++){
    table_rV[i] = table_r + entry_size * div_round (crv * (i-128), 76309);
    table_gU[i] = table_g + entry_size * div_round (cgu * (i-128), 76309);
    table_gV[i] = entry_size * div_round (cgv * (i-128), 76309);
    table_bU[i] = table_b + entry_size * div_round (cbu * (i-128), 76309);
  }
}

void YuvToRgb(rgb *dest,uyvy *src,int w,int h)
{
  int i,s;
  int U, V, Y;
  uyvy p;
  rgb c;
  unsigned char * r, * g, * b;
  s = w * h;
  for(i=0; i<s; i++){
    p = src[i / 2];

    U = p.u;				
    V = p.v;

    r = table_rV[V];
    g = table_gU[U] + table_gV[V];
    b = table_bU[U];

    Y = p.y1;
    c.red = r[Y];
    c.green = g[Y];
    c.blue = b[Y];
    dest[i] = c;

    Y = p.y2;
    c.red = r[Y];
    c.green = g[Y];
    c.blue = b[Y];
    dest[i+1] = c;
  }
}

//====================================================================//

#ifdef CAPTURE

rgb YuvToRgb(uyvy p)
{
  rgb r;
  int y,u,v;

  y = 582 * (p.y1 + p.y2 - 32);
  u = p.u - 128;
  v = p.v - 128;
  r.red   = bound((y + 1596*u         ) / 1000,0,255);
  r.green = bound((y -  813*u -  391*v) / 1000,0,255);
  r.blue  = bound((y          + 2018*v) / 1000,0,255);

  return(r);
}

rgb YuvToRgb(yuv p)
{
  rgb r;

  r.red   = bound((p.y + 1596*p.u         ) / 1000,0,255);
  r.green = bound((p.y -  813*p.u -  391*p.v) / 1000,0,255);
  r.blue  = bound((p.y            + 2018*p.v) / 1000,0,255);

  return(r);
}

void TestHistogram()
{
  int y,u,v;
  int yd,ud,vd;
  uyvy p;
  rgb r;

  yd = 256 / hist.getSizeZ();
  ud = 256 / hist.getSizeY();
  vd = 256 / hist.getSizeX();

  for(y=0; y<255; y+=yd){
    for(u=0; u<255; u+=ud){
      for(v=0; v<255; v+=vd){
	p.y1 = p.y2 = y + yd/2;
	p.u = u + ud/2;
	p.v = v + vd/2;
	r = YuvToRgb(p);
	hist.set(p.v / vd, p.u / ud, p.y2 / yd, r);
      }
    }
  }
}

void InitCapture()
{
  image_width  = IMAGE_WIDTH;
  image_height = IMAGE_HEIGHT;

  cap.initialize(device,image_width,image_height,V4L2_PIX_FMT_UYVY);
  hist.init(MAX_YSIZE, MAX_USIZE, MAX_VSIZE); 
  hist.set(Rgb::Black);

  printf("Capturing initialized...\n");

  yuv_to_rgb_init();
}

void UpdateHistogram()
{
  uyvy *img,p;
//  rgb r;

  int idx, field;
  int i,size;
//  int y,u,v;
  int yd,ud,vd;

  img = (uyvy*)cap.captureFrame(idx, field);
  memcpy(yuvimg, img, IMAGE_WIDTH * IMAGE_HEIGHT * sizeof(uyvy) / 2);

  yd = 256 / hist.getSizeZ();
  ud = 256 / hist.getSizeY();
  vd = 256 / hist.getSizeX();

  YuvToRgb(rawimg, img, IMAGE_WIDTH, IMAGE_HEIGHT);

  dhistogram.histogram.set(0);

  size = image_width * image_height / 2;
  for(i=0; i<size; i++){
    p = img[i];

    hist.set(p.v/vd, p.u/ud, p.y1/yd, rawimg[2*i + 0]);
    hist.set(p.v/vd, p.u/ud, p.y2/yd, rawimg[2*i + 1]);

    int d = dhistogram.histogram.get(p.v/vd, p.u/ud, p.y1/yd);
    dhistogram.histogram.set(p.v/vd, p.u/ud, p.y1/yd, d + 1);
    d = dhistogram.histogram.get(p.v/vd, p.u/ud, p.y2/yd);
    dhistogram.histogram.set(p.v/vd, p.u/ud, p.y2/yd, d + 1);
			     

    colorimg[2*i + 0] = color[dhistogram.tmap.get(p.v/vd, p.u/ud, p.y1/yd)].color;
    colorimg[2*i + 1] = color[dhistogram.tmap.get(p.v/vd, p.u/ud, p.y2/yd)].color;
  }

  cap.releaseFrame((uchar*)img, idx);
}

yuv GetLocation(int x,int y, rgb &c)
{
  uyvy p;
  yuv r;
//  int idx;

  // retrieve the colors
  p = yuvimg[(y*image_width + x)/2];
  r.y = (x&1)? p.y2 : p.y1;
  r.u = p.u;
  r.v = p.v;

  c = rawimg[(y*image_width + x)];
  return(r);
}

yuv TMapLoc(yuv p)
{
  yuv r;
  int dy,du,dv;

  dy = 256 / MAX_YSIZE;
  du = 256 / MAX_USIZE;
  dv = 256 / MAX_VSIZE;

  r.y = p.y / dy;
  r.u = p.u / du;
  r.v = p.v / dv;

  return(r);
}

void StopCapture()
{
  cap.close();
}

#else
void InitCapture() {}
void UpdateHistogram() {}
void StopCapture() {}
#endif

